#ifndef KALMAN_FILTER_2D_HPP
#define KALMAN_FILTER_2D_HPP

#include <opencv2/opencv.hpp>
#include <ros/ros.h>

class KalmanFilter2D {
private:
    cv::KalmanFilter kf;
    bool initialized;
    float max_jump_distance;

public:
    KalmanFilter2D() : initialized(false), max_jump_distance(50.0f) {
        kf.init(4, 2, 0); // 状态向量(x,y,dx,dy), 测量向量(x,y)
        
        // 状态转移矩阵
        kf.transitionMatrix = (cv::Mat_<float>(4, 4) << 
            1, 0, 1, 0,
            0, 1, 0, 1,
            0, 0, 1, 0,
            0, 0, 0, 1);
            
        // 测量矩阵
        cv::setIdentity(kf.measurementMatrix);
        kf.measurementMatrix = (cv::Mat_<float>(2, 4) <<
            1, 0, 0, 0,
            0, 1, 0, 0);
            
        // 过程噪声协方差
        cv::setIdentity(kf.processNoiseCov, cv::Scalar::all(0.1));
        
        // 测量噪声协方差
        cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(0.1));
        
        // 后验错误协方差
        cv::setIdentity(kf.errorCovPost, cv::Scalar::all(1));
    }

    // 设置最大跳变距离
    void setMaxJumpDistance(float distance) {
        max_jump_distance = distance;
    }

    // 更新过程噪声
    void setProcessNoise(float noise) {
        cv::setIdentity(kf.processNoiseCov, cv::Scalar::all(noise));
    }

    // 更新测量噪声
    void setMeasurementNoise(float noise) {
        cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(noise));
    }

    cv::Point2f update(const cv::Point2f& measurement) {
        if (!initialized) {
            kf.statePost.at<float>(0) = measurement.x;
            kf.statePost.at<float>(1) = measurement.y;
            kf.statePost.at<float>(2) = 0;
            kf.statePost.at<float>(3) = 0;
            initialized = true;
            return measurement;
        }

        cv::Mat prediction = kf.predict();
        cv::Point2f predicted_pt(prediction.at<float>(0), prediction.at<float>(1));

        // 计算测量值与预测值之间的距离
        float distance = cv::norm(predicted_pt - measurement);
        
        // 如果距离太大，可能是异常值
        // if (distance > max_jump_distance) {
        //     ROS_WARN("检测到异常跳变! 距离: %.2f, 使用预测值", distance);
        //     return predicted_pt;  // 返回预测值而不是测量值
        // }

        // 更新测量值
        cv::Mat measurement_mat = (cv::Mat_<float>(2, 1) << measurement.x, measurement.y);
        cv::Mat estimated = kf.correct(measurement_mat);
        
        return cv::Point2f(estimated.at<float>(0), estimated.at<float>(1));
    }

    void reset() {
        initialized = false;
    }
};

#endif // KALMAN_FILTER_2D_HPP